// Copyright (C) 2010, Scott W. Dunlop <swdunlop@gmail.com>
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//    * Neither the name of the <organization> nor the
//      names of its contributors may be used to endorse or promote products
//      derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#define NOSPEX_IMPL_SOURCE // Indicates that this is an implementation file.

#include "nospex.h"
#include "util.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

//-------------------------------------------------------- PRIVATE DECLARATIONS

// An arbitrary decimal scaling factor that is proportional to the force
// separating nodes.

#define NX_NODE_REPULSION_FACTOR 1.0

// An arbitrary decimal scaling factor that is proportional to the force
// attracting linked nodes to each other.

#define NX_LINK_CONTRACTION_FACTOR 0.005

// A decay for velocity to consume inertia.

#define NX_VELOCITY_DAMPER 0.90

// The minimum net motion that requires recalculation of forces.

#define NX_EQUILIBRUM_THRESHOLD 0.0000001

// The ideal link length.

#define NX_IDEAL_LINK_LENGTH 5.0

// Maximum number of milliseconds we'll let the physics coroutine run after a
// disturbance to the graph.

#define NX_PHYSICS_MS 10000

#define NX_MAX_FORCE 30.0
#define NX_MAX_VELOCITY 01.0

void nx_calc_blip_position( nx_blip_p blip, nx_time now, double* x, double* y ){
    double ox, oy, dx, dy, p;
    nx_node_p src = blip->link->src;
    nx_node_p dst = blip->link->dst;

    if( now >= blip->expiry ){
        if( x ) *x = blip->link->dst->x; 
        if( y ) *y = blip->link->dst->y;
    }else{
        if( blip->origin == NX_FROM_DST ){
            ox = dst->x;
            oy = dst->y;
            dx = src->x - ox;
            dy = src->y - oy;
        }else{
            ox = src->x;
            oy = src->y;
            dx = dst->x - ox;
            dy = dst->y - oy;
        };

        p = (blip->expiry - now) * blip->pitch;

        if( x ) *x = ox + dx * p;
        if( y ) *y = oy + dy * p;
    };
}

/** Calculate the mutual repulsion of two nodes using weight k. */
void nx_calc_node_force( nx_node_p m, nx_node_p n, double k ){
    /* Implementation Notes:
       - This is patterned on electrostatic repulsion, with some shortcutting
         involved in k, which is proportional to the time interval to be added
         to the velocity.
       - Other shortcuts:
         - There is no terminal velocity.
         - We are working with point masses of uniform charge and mass.
         - All nodes have the same friction coefficient, static and mobile.
         - Time progresses in quanta whose magnitude corresponds to cpu load.
         - All swallows are assumed to be unladen, African, with a tailwind.
     *//* 
           a       d   c     * node M is at point a.
            +------+--+      * node N is at point b.
             \     |  |      * line ab is the distance between M and N, Q
              \    |  |      * line ac is the dx of Q.
               \   |  |      * line bc is the dy of Q.
                \  |  |      * line ae is the force vector between M and N, V
                 \ |  |      * line ad is the dx of V.
                  \|  |      * line de is the dy of V.
                  e\  |
                    \ |      ab = sqrt( ac * ac + cb * cb )
                     \|      ae = k / (ab * ab)
                      +      ad = ae * ac / ab 
                      b      de = ae * bc / ab 
    */

    double ac  = m->x - n->x;
    double bc  = m->y - n->y;

    double ab2 = (double) ((ac * ac) + (bc * bc));
    if( ab2 == 0.0 ) ab2 = 0.0001;
    double ab  = sqrt( ab2 );
    double ae  = k / ab2;
    if( ae > NX_MAX_FORCE ) ae = NX_MAX_FORCE; 

    double ad  = ( ae * ac / ab ) / 2;
    double de  = ( ae * bc / ab ) / 2;
    
    m->vx += ad; n->vx -= ad;
    m->vy += de; n->vy -= de;
}

/** Calculate the pull of a link on its nodes. */
void nx_calc_link_force( nx_link_p l, double k ){
    /* Implementation Notes:
       - This is patterned on springs which attempt to achieve an ideal 
         distance. All assumptions are shared with nx_calc_node_force.
     *//* 
           a       d   c     * node M is at point a.
            +------+--+      * node N is at point b.
             \     |  |      * line ab is the distance between M and N, Q
              \    |  |      * line ac is the dx of Q.
               \   |  |      * line bc is the dy of Q.
                \  |  |      * line ae is the force vector between M and N, V
                 \ |  |      * line ad is the dx of V.
                  \|  |      * line de is the dy of V.
                  e\  |
                    \ |      ab = sqrt( ac * ac + cb * cb )
                     \|      ae = k / (ab * ab)
                      +      ad = ae * ac / ab 
                      b      de = ae * bc / ab 
    */

    nx_node_p m = l->src;
    nx_node_p n = l->dst;

    double   ac  = m->x - n->x;
    double   bc  = m->y - n->y;

    double ab2 = (double) ((ac * ac) + (bc * bc));
    double ab  = (ab2 == 0.0) ? 0.01 : sqrt( ab2 );
    double ae  = k * ( NX_IDEAL_LINK_LENGTH - ab );
    if( ae > NX_MAX_FORCE ) ae = NX_MAX_FORCE; 
    if( ae < -NX_MAX_FORCE ) ae = NX_MAX_FORCE; 

    if( ae == 0.0 ) return; // Spring-local equilibrium achieved.

    double ad  = ( ae * ac / ab ) / 2;
    double de  = ( ae * bc / ab ) / 2;
    
    m->vx += ad; n->vx -= ad;
    m->vy += de; n->vy -= de;
}

/** Calculate the mutual repulsion of all nodes in the graph. Used by graph
    physics. */
void nx_calc_graph_node_forces( nx_graph_p graph, double k ){
    nx_node_p m, n;
    
    for( m = graph->first_node; m != NULL; m = NEXT( m ) ){ 
        for( n = NEXT( m ); n != NULL; n = NEXT( n ) ){
            nx_calc_node_force( m, n, k );
        }
    } 
}

/** Calculate the link contraction of all links in the graph. Used by graph
    physics. */
void nx_calc_graph_link_forces( nx_graph_p graph, double k ){
    nx_link_p link;
    
    FOREACH_LINK( graph, link ){ 
        nx_calc_link_force( link, k );
    } 
}

double nx_clamp_velocity( double velo ){
    if( velo < -NX_MAX_VELOCITY ){
        velo = -NX_MAX_VELOCITY;
    }else if( velo > NX_MAX_VELOCITY ){
        velo = NX_MAX_VELOCITY;
    };
    return velo * NX_VELOCITY_DAMPER;
}

double nx_enforce_graph_margins( nx_graph_p graph, int bounce ){
    nx_node_p node; 
    double min = (1.05 * (NX_NODE_RADIUS + 2)) / graph->pitch;
    double max = NX_DOMAIN_SIZE - min;
    double net_motion = 0.0;

    FOREACH_NODE( graph, node ){
        if( node->x > max ){
            net_motion += NX_NODE_RADIUS;
            node->x = max;
            if( bounce ) node->vx *= -0.1; 
        }else if( node->x < min ){
            net_motion += NX_NODE_RADIUS;
            node->x = min;
            if( bounce ) node->vx *= -0.1;
        };

        if( node->y > max ){
            net_motion += NX_NODE_RADIUS;
            node->y = max;
            if( bounce ) node->vy *= -0.1;
        }else if( node->y < min ){
            net_motion += NX_NODE_RADIUS;
            node->y = min;
            if( bounce ) node->vy *= -0.1;
        };
    }
    
    return net_motion;
}

int nx_apply_graph_physics( nx_graph_p graph ){
    nx_time ms, now;
   
    now = nx_graph_clock( graph );

    if( graph->physics_clock > 0 ){
        ms = now - graph->physics_clock;
        if( ms < 2 ) ms = 1;
    }else{
        ms = 5; // Easing the first step.
    };
    
    graph->physics_clock = now;
    nx_node_p n;

    if( (! graph->count ) || (graph->physics_cycles <= 0) ){
        graph->physics_req = NX_HALTED; // Halting due to lack of cycles.
    }else{
        if( ms > graph->physics_cycles ){
            ms = graph->physics_cycles;
        };
        
        graph->physics_cycles -= ms;
    };

    if( graph->physics_req != NX_RUNNING ){
        return 0; // Looks like we are suspended.
    };
    
    if( ms > 30 ) {
        //printf( "Physics lag detected, %i ms.\n", ms );
    };
    ms = 5; // Clamping to work around the shockwave bug.

    nx_calc_graph_node_forces( 
        graph, NX_NODE_REPULSION_FACTOR * ms 
    );

    nx_calc_graph_link_forces( 
        graph, NX_LINK_CONTRACTION_FACTOR * ms
    );
    
    double net_motion = 0;
    nx_element_p p = graph->ptr_elem; 
    FOREACH_NODE( graph, n ) if( (void*)n != (void*)p ){
        net_motion += fabs( n->vx = nx_clamp_velocity( n->vx ) );
        net_motion += fabs( n->vy = nx_clamp_velocity( n->vy ) );
        n->x += n->vx * ms; 
        n->y += n->vy * ms;
    };

    nx_enforce_graph_margins( graph, 1 );

    nx_request_graph_render( graph );
    if( net_motion <= ( NX_EQUILIBRUM_THRESHOLD * ms ) ){
        //printf( "Equilibrium has been achieved.\n" );
        
        graph->physics_cycles = 0;
        graph->physics_req = NX_HALTED;
        nx_graph_redispatch_pointer( graph );
        return 0;
    }else{
        nx_graph_redispatch_pointer( graph );
        return 1;
    };
}

int nx_apply_graph_blips( nx_graph_p graph ){
    if( graph->blips_req != NX_RUNNING ) return 0;
    if( graph->first_blip == NULL ){
        graph->blips_req = NX_HALTED;
        return 0;
    };
    nx_graph_redispatch_pointer( graph );
    nx_request_graph_render( graph );
    return 1;
}

void nx_resume_graph_blips( nx_graph_p graph ){
    if( graph->blips_req == NX_PAUSED ) nx_schedule_graph_blips( graph );
    if( graph->paused ) graph->blips_req = NX_PAUSED;
}

void nx_suspend_graph_blips( nx_graph_p graph ){
    if( graph->blips_req != NX_RUNNING ) return;
    graph->blips_req = NX_PAUSED;
}

void nx_request_graph_blips( nx_graph_p graph ){
    if( graph->blips_req == NX_HALTED ){
        nx_schedule_graph_blips( graph );
    };
}

void nx_resume_graph_physics( nx_graph_p graph ){
    if( graph->paused ) return;
    if( graph->physics_cycles == 0 ) return;
    
    nx_schedule_graph_physics( graph ); 
}

void nx_suspend_graph_physics( nx_graph_p graph ){
    if( graph->physics_req != NX_RUNNING ) return;
    graph->physics_req = NX_PAUSED;
}

void nx_request_graph_physics( nx_graph_p graph ){
    graph->physics_cycles = NX_PHYSICS_MS;
    if( graph->paused ) return;

    if( graph->physics_req == NX_HALTED ){
        nx_schedule_graph_physics( graph );
    };
}

void nx_pause_graph( nx_graph_p graph ){
    if( graph->paused ) return;
    graph->paused = nx_system_clock( );
    nx_suspend_graph_physics( graph );
    nx_request_graph_render( graph );
}

void nx_resume_graph( nx_graph_p graph ){
    if( ! graph->paused ) return;
    graph->clock_skew += nx_system_clock( ) - graph->paused;
    graph->paused = 0;

    nx_resume_graph_physics( graph );
    nx_request_graph_render( graph );
}
